#ifndef __MAP_HELPHER_H__
#define __MAP_HELPHER_H__

#include <iostream>
#include <sstream>
#include <opencv2/opencv.hpp>
using namespace std;
using namespace cv;

class MapHelper{
    public:
        // 地图像素值转世界坐标
        cv::Point2f Map2World(cv::Point point, cv::Point2f origin_point, float resolution){
            cv::Point2f world_point;
            world_point.x = point.x * resolution + origin_point.x; 
            world_point.y = point.y * resolution + origin_point.y;
            return world_point;
        }

        // 世界坐标转地图像素值     ROS地图下
        cv::Point Map2Pixel(cv::Point2f world_point, cv::Point2f origin_point, float resolution){
            cv::Point map_point;
            float resolution_inverse = 1.0 / resolution;
            map_point.x = (world_point.x - origin_point.x) * resolution_inverse; 
            map_point.y = (world_point.y - origin_point.y) * resolution_inverse;
            return map_point;
        }

        // 世界坐标转地图像素值  Opencv地图下
        cv::Point World2OpencvPixel(cv::Point2f world_point, cv::Point2f origin_point, float resolution, int height){
            cv::Point map_point = Map2Pixel(world_point, origin_point, resolution);
            map_point.y = height - map_point.y;
            return map_point;
        }

        // 地图像素值转世界坐标  Opencv地图下
        cv::Point2f OpencvPixel2World(cv::Point point, cv::Point2f origin_point, float resolution, int height){
            point.y = height - point.y;
            return Map2World(point, origin_point, resolution);
        }

    
};
#endif